#include "axisgroup_robot.h"
#include "log/zuclog.h"
#include "force_control.h"
#include "axis/axis.h"
#include "rtdev/servo/servoitf.h"
#include "dyna/jaka_dyn_intf.h"
#include "kine/kine.h"
#include "axisgroup_set.h"
#include "rob_common/robtool.h"
#include "rob_common/usrframe.h"
#include "over_plan.h"
#include "rt_servo_plan.h"
#include <math.h>

#define AXISGROUP_LOG_INFO(fmt, ...) zuclog_info("AxisGroup %d", fmt, group_id_, ##__VA_ARGS__)
#define AXISGROUP_LOG_ERR(fmt, ...) zuclog_error("AxisGroup %d", fmt, group_id_, ##__VA_ARGS__)

#define DIFF(A, B) (A - B) / (config_.cyclictime_sec)
#define POSE_DIST(A, B) \
    sqrt((A.tran.x - B.tran.x) * (A.tran.x - B.tran.x) + (A.tran.y - B.tran.y) * (A.tran.y - B.tran.y) + (A.tran.z - B.tran.z) * (A.tran.z - B.tran.z))

using namespace axisgroup;

axisgroup::Robot::Robot(uint32_t serial_num) : axisgroup::AxisGroup(serial_num)
{
    init_pfsm();
    target_plan_mode_ = PFsmState::PlanMode::PLAN_IDLE;
    over_plan_ = std::make_shared<OverPlan>(this);
}

axisgroup::Robot::~Robot() { AXISGROUP_LOG_INFO("Robot be destroyed"); }

void axisgroup::Robot::update()
{
    // 如config发生变化，同步该变化
    set_intf_->update_active_config();

    // 上电、使能状态机, 使能完成后进入standby状态
    update_fsm();

    // 反馈状态
    update_fdb_state();

    // 部分响应事件
    watch_event();

    // 更新规划位置
    PlanStatus state = plan_pfsm_->update();

    // 运动学正解
    kine_->fkine(
        cmd_rst_.jpos, desired_cmd_.pose, &(RobotToolMgr::get_rob_tool(tool_id_).pose()), &(UsrFrameMgr::get_usr_frame(wobj_id_).world(base_frame_id_)));

    // 偏移， 如果叠加被开启，则视为moving状态？
    int ret = update_pose_offset();

    // 逆解
    if (ret)
    {
        // 原始位置被改变，需要重新作正逆解
    }

    if (state.errcode != 0)
    {
        // 错误处理
        change_to_errstat((RobEvCode)state.errcode, 0);
    }
    else
    {
        // 根据规划状态切换fsm状态机
        update_fsm_mode(state);
    }

    // 安全检查
    // check_safezone();

    // 规划状态机切换, 必须在update_fsm_mode后
    update_plan_mode();

    // 指令状态计算，包含指令力矩的计算
    update_cmd_state();

    // 伺服输出, 输出单位为度，Nm
    bool is_jump = false;
    if (pos_jump_)
    {
        // pos_jump_ 1. servo 模式 abort 的周期置位
        pos_jump_ = false;
        is_jump = true;
    }
    for (size_t i = 0; i < joint_num(); i++) { joints[i]->sync_motion_plan(moving_state_.cmd.cmd_pos[i], moving_state_.cmd.cmd_torque[i], is_jump); }
    // AXISGROUP_LOG_INFO("curr cmd %s, %s", cmd_rst_.jpos.val_str().c_str(), moving_state_.cmd.cmd_pos.val_str().c_str());
}

void axisgroup::Robot::switch_plan_mode(PFsmState::PlanMode mode)
{
    target_plan_mode_ = mode;
    update_plan_mode();
}

PFsmState::PlanMode axisgroup::Robot::get_cur_plan_mode() const { return plan_pfsm_->plan_type_; }

bool axisgroup::Robot::is_inpos() { return plan_pfsm_->is_inpos(); }

void axisgroup::Robot::motion_abort()
{
    AXISGROUP_LOG_INFO("motion abort");
    free_pfsm_->abort_plan();
    coord_pfsm_->abort_plan();
    drag_pfsm_->abort_plan();
    servo_pfsm_->abort_plan();
}

void axisgroup::Robot::pause() { plan_pfsm_->pause(); }

void axisgroup::Robot::resume() { plan_pfsm_->resume(); }

void axisgroup::Robot::init_pfsm()
{
    free_pfsm_ = creat_pfsm(this, PFsmState::PlanMode::PLAN_FREE);
    servo_pfsm_ = creat_pfsm(this, PFsmState::PlanMode::PLAN_SERVO);
    drag_pfsm_ = creat_pfsm(this, PFsmState::PlanMode::PLAN_DRAG);
    coord_pfsm_ = creat_pfsm(this, PFsmState::PlanMode::PLAN_COORD);
    idle_pfsm_ = creat_pfsm(this, PFsmState::PlanMode::PLAN_IDLE);

    plan_pfsm_.reset();
    plan_pfsm_ = idle_pfsm_;
}

void axisgroup::Robot::update_fsm_mode(PlanStatus state)
{
    if (get_state() == FsmState::Booting || get_state() == FsmState::Disabled || get_state() == FsmState::ErrorStop)
    {
        return;
    }
    switch (state.status)
    {
    case PlanMovingStatus::MOVING_RUNNING:
        change_fsm_stat(FsmState::Moving);
        break;
    case PlanMovingStatus::MOVING_STOPPING:
    case PlanMovingStatus::MOVING_ERRSTOPPING:
        change_fsm_stat(FsmState::Stopping);
        break;
    case PlanMovingStatus::MOVING_STOPED:
        change_fsm_stat(FsmState::Standby);
        break;
    default:
        break;
    }
}

int axisgroup::Robot::reset_kine_dyn()
{
    AxisGroup::reset_kine_dyn();
    coord_pfsm_->init();  // TP需求运动学和动力学接口
    return 0;
}

void axisgroup::Robot::update_fdb_state()
{
    MovingState last_state = moving_state_;
    for (size_t i = 0; i < joint_num(); i++)
    {
        joints[i]->update_joint_fdb_status(rtrobot_status_.joint_status[i]);  // 刷新axis的状态, rtrobot_status_由rtrobot刷新而来

        moving_state_.fdb.fdb_pos[i] = joints[i]->status_.act_pos;
        moving_state_.fdb.fdb_vel[i] = DIFF(moving_state_.fdb.fdb_pos[i], last_state.fdb.fdb_pos[i]);
        moving_state_.fdb.fdb_acc[i] = DIFF(moving_state_.fdb.fdb_vel[i], last_state.fdb.fdb_vel[i]);
        moving_state_.fdb.fdb_jerk[i] = DIFF(moving_state_.fdb.fdb_acc[i], last_state.fdb.fdb_acc[i]);

        moving_state_.fdb.fdb_torque[i] = joints[i]->status_.act_torq;  // 伺服实际的反馈位置
        moving_state_.servo_cmd_pos[i] = joints[i]->status_.cmd_pos;    // 伺服实际的指令为止
    }
    moving_state_.fdb.linear_fdb_vel = POSE_DIST(moving_state_.fdb.fdb_pose, last_state.fdb.fdb_pose) / config_.cyclictime_sec;
    kine_->fkine(moving_state_.fdb.fdb_pos,
                 moving_state_.fdb.fdb_pose,
                 &(RobotToolMgr::get_rob_tool(tool_id_).pose()),
                 &(UsrFrameMgr::get_usr_frame(wobj_id_).world(base_frame_id_)));

    // 力传感器反馈数据
    // ftc_->update();
}

void axisgroup::Robot::update_cmd_state()
{
    MovingState last_state = moving_state_;
    for (size_t i = 0; i < joint_num(); i++)
    {
        moving_state_.cmd.cmd_pos[i] = cmd_rst_.jpos[i];
        moving_state_.cmd.cmd_vel[i] = DIFF(moving_state_.cmd.cmd_pos[i], last_state.cmd.cmd_pos[i]);
        moving_state_.cmd.cmd_acc[i] = DIFF(moving_state_.cmd.cmd_vel[i], last_state.cmd.cmd_vel[i]);
        moving_state_.cmd.cmd_jerk[i] = DIFF(moving_state_.cmd.cmd_acc[i], last_state.cmd.cmd_acc[i]);
    }
    moving_state_.cmd.cmd_pose = cmd_rst_.pose;
    moving_state_.cmd.linear_cmd_vel = POSE_DIST(moving_state_.cmd.cmd_pose, last_state.cmd.cmd_pose) / config_.cyclictime_sec;
    moving_state_.cmd.linear_cmd_acc = DIFF(last_state.cmd.linear_cmd_vel, moving_state_.cmd.linear_cmd_vel);
#ifdef PRINTF_DEBUG
    AXISGROUP_LOG_INFO("moving vel = %s", moving_state_.cmd.cmd_vel.val_str().c_str());
#endif
    update_cmd_torque();
}

void axisgroup::Robot::update_cmd_torque()
{
    double q[MAX_AXIS] = {};
    double qd[MAX_AXIS] = {};
    double qdd[MAX_AXIS] = {};
    ROBOT_JOINT_DEG_TO_RAD(q, moving_state_.cmd.cmd_pos);
    ROBOT_JOINT_DEG_TO_RAD(qd, moving_state_.cmd.cmd_vel);
    ROBOT_JOINT_DEG_TO_RAD(qdd, moving_state_.cmd.cmd_acc);
    // #TODO to be update
    if (get_cur_plan_mode() == PFsmState::PlanMode::PLAN_DRAG)
    {
        // 计算转矩, 拖拽模式需要1ms周期进行计算
        RobJointVal friction;
        dyna_->getJointGf(q, qd, moving_state_.cmd.cmd_torque.val, friction.val);
    }
    else if (get_cur_plan_mode() == PFsmState::PlanMode::PLAN_IDLE)
    {
        // IDLE 不动，任何指令的变化也不是规划出来的
        memset(qd, 0, sizeof(double) * MAX_AXIS);
        memset(qdd, 0, sizeof(double) * MAX_AXIS);
        dyna_->getTa(q, qd, qdd, moving_state_.cmd.cmd_torque.val);
    }
    else
    {
        dyna_->getTa(q, qd, qdd, moving_state_.cmd.cmd_torque.val);
    }
}

// 力控叠加偏移或其他偏移
int axisgroup::Robot::update_pose_offset()
{
    if (!ftc_.get())
    {
        ftc_ = std::make_shared<axisgroup::ForceControlIntf>(this);
    }
    // 位置偏移
    // int revised = 0;
    // if (grp_->ftc_->commnand_to_close_)
    // {
    //     grp_->ftc_->compliant_enable_state_ = 0;
    // }
    // if (grp_->ftc_->compliant_enable_state_ == 1)
    // {
    //     grp_->ftc_->last_compliant_enable_ = 1;

    //     // 期望位置为用户输入的位置
    //     grp_->desired_cmd_.pose = disired_pos;

    //     ZucPose carte_revised_pos = {};
    //     if (0 != grp_->ftc_->admittanceRevise(disired_pos, grp_->cmd_rst_, carte_revised_pos))
    //     {
    //         // #TODO , 退出sevo模式
    //         return -1;
    //     }

    //     world_admit_revised = carte_revised_pos;
    // }
    // else
    // {
    //     // servo 模式中直接退出力控会存在危险，因此若推出力控，直接报错
    //     if (grp_->ftc_->last_compliant_enable_ == 1)
    //     {
    //         // #TODO , 退出sevo模式
    //         return -1;
    //     }
    //     grp_->ftc_->last_compliant_enable_ = 0;
    //     world_admit_revised = disired_pos;
    // }

    // grp_->ftc_->last_force_control_pose_.pose = world_admit_revised;
    return 0;
}

const char* axisgroup::Robot::get_plan_mode_stirng(PFsmState::PlanMode mode)
{
    switch (mode)
    {
    case PFsmState::PlanMode::PLAN_IDLE:
        return "idle";
    case PFsmState::PlanMode::PLAN_FREE:
        return "free";
    case PFsmState::PlanMode::PLAN_SERVO:
        return "servo";
    case PFsmState::PlanMode::PLAN_DRAG:
        return "drag";
    case PFsmState::PlanMode::PLAN_COORD:
        return "coord";
    default:
        return "unknown";
    }
}

void axisgroup::Robot::update_plan_mode()
{
    if (get_state() == FsmState::Standby)
    {
        if (target_plan_mode_ != get_cur_plan_mode())
        {
            AXISGROUP_LOG_INFO(
                "Plan mode change from %s to %s", PFsmState::PLAN_MODE_NAME[get_cur_plan_mode()].c_str(), PFsmState::PLAN_MODE_NAME[target_plan_mode_].c_str());
            plan_pfsm_->exit_mode();
            plan_pfsm_.reset();
            switch (target_plan_mode_)
            {
            case PFsmState::PlanMode::PLAN_FREE:
                plan_pfsm_ = free_pfsm_;
                break;
            case PFsmState::PlanMode::PLAN_SERVO:
                plan_pfsm_ = servo_pfsm_;
                break;
            case PFsmState::PlanMode::PLAN_DRAG:
                plan_pfsm_ = drag_pfsm_;
                break;
            case PFsmState::PlanMode::PLAN_COORD:
                plan_pfsm_ = coord_pfsm_;
                break;
            default:
                plan_pfsm_ = idle_pfsm_;
                break;
            }
            if (plan_pfsm_->enter_mode())
            {
                // 进入失败 #TODO
                return;
            }
            if (target_plan_mode_ == PFsmState::PlanMode::PLAN_DRAG)
            {
                // 仅拖拽为转矩控制模式
                for (size_t i = 0; i < joints.size(); i++) { joints[i]->set_axis_tracking_mode(axis::Axis::OpMode::OP_MODE_CST); }
            }
            else
            {
                for (size_t i = 0; i < joints.size(); i++) { joints[i]->set_axis_tracking_mode(axis::Axis::OpMode::OP_MODE_CSP); }
            }
        }
        else
        {
            if (get_cur_plan_mode() != PFsmState::PlanMode::PLAN_IDLE)
            {
                // standby状态下，如果没有指令切换为其它模式，就自动切换为IDLE规划
                target_plan_mode_ = PFsmState::PlanMode::PLAN_IDLE;
            }
        }
    }
    else
    {
        if (target_plan_mode_ != get_cur_plan_mode())
        {
            AXISGROUP_LOG_ERR("Cannot change plan mode when robot is not in standby state!");
            target_plan_mode_ = get_cur_plan_mode();  // 非standby状态进来的指令一律清除
        }
        if ((get_state() == FsmState::Disabled || get_state() == FsmState::ErrorStop) && plan_pfsm_->is_inpos() &&
            get_cur_plan_mode() != PFsmState::PlanMode::PLAN_IDLE)
        {
            target_plan_mode_ = PFsmState::PlanMode::PLAN_IDLE;
            AXISGROUP_LOG_INFO("Plan mode change from %s to %s because of disabled!",
                               PFsmState::PLAN_MODE_NAME[get_cur_plan_mode()].c_str(),
                               PFsmState::PLAN_MODE_NAME[target_plan_mode_].c_str());
            // 机器人下使能状态，切换至IDLE，指令位置跟随反馈位置
            plan_pfsm_->exit_mode();
            plan_pfsm_.reset();
            plan_pfsm_ = idle_pfsm_;
            plan_pfsm_->enter_mode();
            for (size_t i = 0; i < joints.size(); i++) { joints[i]->set_axis_tracking_mode(axis::Axis::OpMode::OP_MODE_CSP); }
        }
    }
}

// 每个traj周期被调用，将task层需要的信息保存下来，在motion线程中被调用
void axisgroup::Robot::prepare_robot_status_for_task()
{
    alive_flg_ = rtrobot_status_.active_status.is_active;  // 所有轴在线

    std::shared_lock<std::shared_mutex> lock(al_data_mtx_);
    robot_status_fdb_.serial_num = serial_num_;
    robot_status_fdb_.rob_id = group_id_;
    robot_status_fdb_.robot_joint_num = axisgroup::RobotSetIntf::get_joint_num_by_serial_num(serial_num_);
    robot_status_fdb_.is_alive = is_alive();  // 任意轴在线
    robot_status_fdb_.is_sim = is_sim();
    robot_status_fdb_.is_power_on = is_powered();
    robot_status_fdb_.is_powering = is_powering();
    robot_status_fdb_.is_enable = is_enabled();
    robot_status_fdb_.is_enabling = is_enabling();
    robot_status_fdb_.is_drag = (get_cur_plan_mode() == PFsmState::PlanMode::PLAN_DRAG) ? true : false;
    robot_status_fdb_.is_error = (fsm_sta_ == FsmState::ErrorStop) ? true : false;

    robot_status_fdb_.cmd_jpos = moving_state_.cmd.cmd_pos;           // motion层规划位置
    robot_status_fdb_.cmd_servo_jpos = moving_state_.servo_cmd_pos;   // 伺服最终下发的位置
    robot_status_fdb_.act_jpos = moving_state_.fdb.fdb_pos;           // 反馈关节位置
    robot_status_fdb_.act_tcp_pose = moving_state_.fdb.fdb_pose;      // 反馈tcp位置
    robot_status_fdb_.desired_tcp_pose = moving_state_.cmd.cmd_pose;  // motion 层规划的
    robot_status_fdb_.cmd_tcp_pose = moving_state_.cmd.cmd_pose;      // 伺服实际算出来的 #TODO 减少重复正运动学计算
    robot_status_fdb_.cmd_tor = moving_state_.cmd.cmd_torque;         // 规划的指令力矩
    robot_status_fdb_.act_tor = moving_state_.fdb.fdb_torque;         // 实际的力矩

    robot_status_fdb_.is_act_inpos = plan_pfsm_->is_inpos();  // 规划停止，反馈到位
    robot_status_fdb_.rapid_scale = config_.rapid_scale;      // 速度倍率
    robot_status_fdb_.is_paused = plan_pfsm_->is_paused();

    robot_status_fdb_.plan_mode = (int)get_cur_plan_mode();
    // robot_status_fdb_.is_cmd_done = ;
    // robot_status_fdb_.is_error = ;      //是否有错误

    robot_status_fdb_.cur_tool_id = tool_id_;
    robot_status_fdb_.cur_usrframe_id = wobj_id_;
    robot_status_fdb_.base_frame_id = base_frame_id_;
    robot_status_fdb_.robot_dh = kine_->Robot_Conf.robot_dh;     // 对外显示运动学库使用的真实DH
    robot_status_fdb_.robot_config = set_intf_->active_config_;  // 激活的机器人配置
}

// 每个task周期被调用，在TASK线程中被调用
void axisgroup::Robot::get_robot_status_for_task(mot::al::RobotALIntf::Status* status)
{
    std::shared_lock<std::shared_mutex> lock(al_data_mtx_);
    memcpy(status, &robot_status_fdb_, sizeof(mot::al::RobotALIntf::Status));
    // *status = robot_status_fdb_;
}

void axisgroup::Robot::watch_event()
{
    // 回零事件
    if (is_homing())
    {
        if (is_homed() && cmd_.home_joint > -1)  // 是否home到位是根据伺服状态字8、10、12位判读的
        {
            // 回零结束，指令需要跟随反馈变动
            cmd_rst_.jpos[cmd_.home_joint] = moving_state_.fdb.fdb_pos[cmd_.home_joint];
            if (cmd_.home_wait_count)
            {
                // 等待几个周期
                cmd_.home_wait_count--;
                AXISGROUP_LOG_INFO("home_wait_count %d, cur cmd = %lf", cmd_.home_wait_count, cmd_rst_.jpos[cmd_.home_joint]);
            }
            else
            {
                servo_home_end_action();  // 设置回位置模式，home_joint复位
            }
        }
    }
}

/// @brief 该接口与在rtbus线程中以rtbus的周期被调用
void axisgroup::Robot::rt_over_plan_update(double ts)
{
    AxisGroupPose pos_desired;                                                                            // 规划的目标位置
    for (size_t i = 0; i < joints.size(); i++) { joints[i]->update_servo_cmd(ts, pos_desired.jpos[i]); }  // 对motion层的位置进行插补
    kine_->fkine(
        pos_desired.jpos, pos_desired.pose, &(RobotToolMgr::get_rob_tool(tool_id_).pose()), &(UsrFrameMgr::get_usr_frame(wobj_id_).world(base_frame_id_)));
    // AXISGROUP_LOG_INFO("now: ds = %lf, pos = %s", ts, pos_desired.jpos.val_str().c_str());

    AxisGroupPose servo_plan_target;
    if (over_plan_->is_servo_active())
    {
        // servo 模式叠加
        over_plan_->rt_plan_servo_->update(pos_desired, servo_plan_target);
    }
    else
    {
        servo_plan_target = pos_desired;
    }

    if (over_plan_->is_fct_active())
    {
        // 力控 模式叠加
        // over_plan_->force_control->update(servo_plan_target, ftc_plan_target);
    }
    // AXISGROUP_LOG_INFO("after: ds = %lf, pos = %s", ts, servo_plan_target.jpos.val_str().c_str());
    for (size_t i = 0; i < joints.size(); i++) { joints[i]->sync_servo_move(servo_plan_target.jpos[i], 0); }  // TODO LW
}